import rclpy
from rclpy.node import Node
from tf2_ros import TransformListener, Buffer
import tf2_ros

class TF2Listener(Node):
    def __init__(self):
        super().__init__('tf2_listener')
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        self.timer = self.create_timer(0.1, self.get_transform)

    def get_transform(self):
        try:
            # 查询 child_frame 相对于 parent_frame 的变换
            trans = self.tf_buffer.lookup_transform('parent_frame', 'child_frame', rclpy.time.Time())
            self.get_logger().info(f"Translation: {trans.transform.translation}")
            self.get_logger().info(f"Rotation: {trans.transform.rotation}")
        except tf2_ros.LookupException:
            self.get_logger().warn("Transform not available yet")

def main(args=None):
    rclpy.init(args=args)
    node = TF2Listener()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
